#ifndef KALMAN_H
#define KALMAN_H

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <vector>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>

using namespace Eigen;
using namespace std;

class Kalman
{
    public:
        Kalman();
        virtual ~Kalman();
        Vector2f process(float measure,float accz);
/*
        Matrix<float, 2, 1> LPF(Matrix<float, 2, 1> measure);
        Matrix<float, 2, 1> AVG(Matrix<float, 2, 1> measure);
        Matrix<float, 2, 1> MOVING_AVG(Matrix<float, 2, 1> measure);
*/
        void init();
        Matrix2f m_A,m_Q,m_P;
        Matrix<float, 1, 2> m_H;
        Matrix<float, 2, 1> m_B;
        float m_R;
        Vector2f m_x;
        bool m_KM_initial;
        bool m_LPF_initial;
        bool m_AVG_initial;
        bool m_MOVING_AVG_initial;
        int m_total_num_samp;
        int m_moving_num_samp;
        FILE* fp_cov;
	float ts;

/*
        Matrix<float, 2, 1> prevX_lpf,lpf_x;
        Matrix<float, 2, 1> prevX_avg,avg_x;
        Matrix<float, 2, 1> prevX_moving,moving_x;
*/
    private:
        
};

#endif
